#include "Game Engine\Header\Airplane.h"

Airplane::Airplane( double x, double y, double z, 
	double vx, double vy, double vz, double time,
	double wingArea, double wingSpan, double tailArea,
	double clSlope0, double cl0, double clSlope1,
	double cl1, double alphaClMax,
	double cdp, double eff, double mass, 
	double enginePower, double engineRps,
	double propDiameter, double a, double b )
{
	mS =	time;       //  time = 0.0
	mQ[0] = vx;   //  vx0 = 0.0
	mQ[1] = x;   //  x0 = 0.0
	mQ[2] = vy;   //  vy0 = 0.0
	mQ[3] = y;   //  y0 = 0.0
	mQ[4] = vz;   //  vz0 = 0.0
	mQ[5] = z;   //  z0 = 0.0

	mWingArea = wingArea;
	mWingSpan = wingSpan;
	mTailArea = tailArea;
	mClSlope0 = clSlope0;
	mCl0 = cl0;
	mClSlope1 = clSlope1;
	mCl1 = cl1;
	mAlphaClMax = alphaClMax;
	mCdp = cdp;
	mEff = eff;
	mMass = mass;
	mEnginePower = enginePower;
	mEngineRps = engineRps;
	mPropDiameter = propDiameter;
	mA = a;
	mB = b;

	mGroundDistance = 0.0f;
	mGroundPosition = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );
	mGroundRotation = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );

	//  Initially, set bank, angle of attack, 
	//  and throttle to zero
	mFlap = "0";         //  Flap setting

	mNumEqns = 6;

	mBank = 0.0;
	mAlpha = 0.0;
	mThrottle = 0.0;
	mFlap = "0";
}

void Airplane::Init( ID3D10Device* device, btDynamicsWorld* world, std::wstring chassis,
	std::wstring wheels, std::wstring ground, ID3D10Effect* fx, ID3D10ShaderResourceView* cubeMap, float mass,
	bool useShadows )
{
	btTransform tr;
	tr.setIdentity();

	// Create the chassis based on the size of the mesh
	D3DXVECTOR3 chassisMin;
	D3DXVECTOR3 chassisMax;

	mChassisMesh = new Mesh;
	mChassisMesh->Init( device, chassis, fx, useShadows );
	mChassisMesh->SetCubeMap( cubeMap );

	mChassisMesh->GetAABB( chassisMin, chassisMax );

	mChassisWidth = abs( chassisMax.x - chassisMin.x ) / 2;
	mChassisHeight = abs( chassisMax.y - chassisMin.y ) / 2;
	mChassisLength = abs( chassisMax.z - chassisMin.z ) / 2;

	btCollisionShape* chassisShape = new btBoxShape( btVector3( mChassisWidth,
	mChassisHeight, mChassisLength ) );

	if( wheels != L"" )
	{
		btCompoundShape* compound = new btCompoundShape();
		btTransform localTrans;
		localTrans.setIdentity();

		//localTrans effectively shifts the center of mass with respect to the chassis
		localTrans.setOrigin( btVector3( 0.0f, mChassisHeight * 2.0f, 0.0f ) );

		compound->addChildShape( localTrans, chassisShape );

		tr.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) );
		mPlaneChassis = CreateRigidBody( world, mass, tr, compound );//chassisShape);
	}
	else
	{
		tr.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) );
		mPlaneChassis = CreateRigidBody( world, mass, tr, chassisShape );//chassisShape);
	}

	mGroundMesh = new Mesh();
	mGroundMesh->Init( device, ground, fx, useShadows );

	Vehicle::Init( device, world, chassis, wheels, fx, cubeMap, mMass, useShadows );
}

void Airplane::UpdateLocationAndVelocity( double dt )
{
	RungeKutta4( dt );

    double vx = mQ[0];
    double vy = mQ[2];
    double vz = mQ[4];
    double vh = sqrt( vx * vx + vy * vy );
    double airspeed = sqrt( vx * vx + vy * vy + vz * vz );
    double climbAngle;
    double headingAngle;

	if ( vh == 0.0 )
	{
		climbAngle = 0.0;
	}
	else
	{
		climbAngle = atan( vz / vh );
	}

	if ( vx >= 0.0 && vy == 0.0 )
	{
		headingAngle = 0.0;
	}
	else if ( vx == 0.0 && vy > 0.0 )
	{
		headingAngle = 0.5 * PI;
	}
	else if ( vx <= 0.0 && vy == 0.0 )
	{
		headingAngle = PI;
	}
	else if ( vx == 0.0 && vy < 0.0 )
	{
		headingAngle = 1.5 * PI;
	}
	else if ( vx > 0.0 && vy > 0.0 )
	{
		headingAngle = atan( vy / vx );
	}
	else if ( vx < 0.0 && vy > 0.0 )
	{
		headingAngle = 0.5 * PI + atan( abs( vx / vy ) );
	}
	else if ( vx < 0.0 && vy < 0.0 )
	{
		headingAngle = PI + atan( vy / vx );
	}
	else
	{
		headingAngle = 1.5 * PI + atan( abs( vx / vy ) );
	}

	//climbAngleTextBox.Text = ""+(float)( climbAngle * 180.0 / PI);
	//headingTextBox.Text = ""+(float)( headingAngle * 180.0 / PI);
	//climbRateTextBox.Text = ""+(float)vz;
	//altitudeTextBox.Text = ""+(float)GetZ();

	mClimbAngle = (float)( climbAngle * 180.0f / PI );
	mHeadingAngle = (float)( headingAngle * 180.0f / PI );
	mClimbRate = (float)vz;
	mAltitude = (float)mQ[5];
	mAirSpeed = (float)airspeed * 3.6f;
	//btVector3 speed = BTVector3( mLook ) * airspeed * 3.6f;

	//  Airspeed is converted from m/s to km/hr.
	mPosition = BT2DX_VECTOR3( mPlaneChassis->getWorldTransform().getOrigin() );
	D3DXMATRIX pLook = BT2DX_MATRIX( mPlaneChassis->getWorldTransform() );
	mLook.x = pLook._31;
	mLook.y = pLook._32;
	mLook.z = pLook._33;
	//D3DXVec3Normalize( &mLook, &mLook );
	mPlaneChassis->setLinearVelocity( btVector3( vy, vz, vx ) );
	SetRotation( Y, mHeadingAngle, false );
	if( mGroundDistance != 0.0f )
		SetRotation( Z, mBank * 75.0f, false );
	SetRotation( X, mClimbAngle );
	//airspeedTextBox.Text = ""+(float)( airspeed * 3.6 );
}

void Airplane::RungeKutta4( double ds )
{
	int j;
	int numEqns;
	double s;
	double *q;
	double *dq1;
	double *dq2;
	double *dq3;
	double *dq4;

	//  Define a convenience variable to make the
	//  code more readable
	numEqns = mNumEqns;

	//  Allocate memory for the arrays.
	q = (double *)malloc(numEqns*sizeof(double));
	dq1 = (double *)malloc(numEqns*sizeof(double));
	dq2 = (double *)malloc(numEqns*sizeof(double));
	dq3 = (double *)malloc(numEqns*sizeof(double));
	dq4 = (double *)malloc(numEqns*sizeof(double));

	//  Retrieve the current values of the dependent
	//  and independent variables.
	s = mS;
	for(j=0; j<numEqns; ++j)
	{
		q[j] = mQ[j];
	}     

	// Compute the four Runge-Kutta steps, The return 
	// value of planeRightHandSide method is an array
	// of delta-q values for each of the four steps.
	RightHandSide( q, q,   ds, 0.0, dq1	);
	RightHandSide( q, dq1, ds, 0.5, dq2	);
	RightHandSide( q, dq2, ds, 0.5, dq3	);
	RightHandSide( q, dq3, ds, 1.0, dq4	);

	//  Update the dependent and independent variable values
	//  at the new dependent variable location and store the
	//  values in the ODE object arrays.
	mS = mS + ds;

	for(j=0; j<numEqns; ++j)
	{
		q[j] = q[j] + (dq1[j] + 2.0*dq2[j] + 2.0*dq3[j] + dq4[j])/6.0;
		mQ[j] = q[j];
	}

	D3DXVECTOR3 p = mPosition;
	GetGround();
	mQ[5] = mGroundDistance;
	if( mQ[5] <= -1000.0f )
		mQ[5] = 1000.0f;

	//  Free up memory
	free(q);
	free(dq1);
	free(dq2);
	free(dq3);
	free(dq4);

	return;
}

//*************************************************************
//  This method loads the right-hand sides for the plane ODEs
//*************************************************************
void Airplane::RightHandSide( double *q, double *deltaQ, double ds, 
                             double qScale, double *dq )
{
	//  q[0] = vx = dxdt
	//  q[1] = x
	//  q[2] = vy = dydt
	//  q[3] = y
	//  q[4] = vz = dzdt
	//  q[5] = z
	double newQ[6]; // intermediate dependent variable values.

	int i;
	double pi = acos(-1.0);
	double G = -9.81;

	double vx = 0.0f;
	double vy = 0.0f;
	double vz = 0.0f;
	double x = 0.0f;
	double y = 0.0f;
	double z = 0.0f;
	double vh = 0.0f;
	double vtotal = 0.0f;
	double temperature = 0.0f;
	double grp = 0.0f;
	double pressure = 0.0f;
	double density = 0.0f;
	double omega = 0.0f;
	double factor = 0.0f;
	double advanceRatio = 0.0f;
	double thrust = 0.0f;
	double cl = 0.0f;
	double lift = 0.0f;
	double aspectRatio = 0.0f;
	double cd = 0.0f;
	double drag = 0.0f;
	double cosW = 0.0f;   //  bank angle
	double sinW = 0.0f;   //  bank angle
	double cosP = 0.0f;   //  climb angle
	double sinP = 0.0f;   //  climb angle
	double cosT = 0.0f;   //  heading angle
	double sinT = 0.0f;   //  heading angle
	double Fx = 0.0f;
	double Fy = 0.0f;
	double Fz = 0.0f;

	//  Convert bank angle from degrees to radians
	//  Angle of attack is not converted because the
	//  Cl-alpha curve is defined in terms of degrees.
	//mBank = mBank*pi/180.0;

	//  Compute the intermediate values of the 
	//  dependent variables.
	for(i=0; i<6; ++i)
	{
		newQ[i] = q[i] + qScale*deltaQ[i];
	}

	//  Assign convenenience variables to the intermediate 
	//  values of the locations and velocities.
	vx = newQ[0];
	vy = newQ[2];
	vz = newQ[4];
	x = newQ[1];
	y = newQ[3];
	z = newQ[5];
	vh = sqrt(vx*vx + vy*vy);
	vtotal = sqrt(vx*vx + vy*vy + vz*vz);

	//  Compute the air density
	temperature = 288.15 - 0.0065*z;
	grp = (1.0 - 0.0065*z/288.15);
	pressure = 101325.0*pow(grp, 5.25);
	density = 0.00348*pressure/temperature;

	//  Compute power drop-off factor
	omega = density/1.225;
	factor = (omega - 0.12)/0.88;

	//  Compute thrust
	advanceRatio = vtotal/(mEngineRps*mPropDiameter);
	thrust = mThrottle*factor*mEnginePower*
			(mA + mB*advanceRatio*advanceRatio)/(mEngineRps*mPropDiameter); 

	//  Compute lift coefficient. The Cl curve is 
	//  modeled using two straight lines.
	if( mAlpha < mAlphaClMax )
	{
		cl = mClSlope0*mAlpha + mCl0;
	}
	else
	{
		cl = mClSlope1*mAlpha + mCl1;
	}

	//  Include effects of flaps and ground effects.
	//  Ground effects are present if the plane is
	//  within 5 meters of the ground.
	if( !strcmp(mFlap,"20") )
	{
		cl += 0.25;
	}
	if( !strcmp(mFlap,"40") )
	{
		cl += 0.5;
	}
	if( z < 5.0 )
	{
		cl += 0.25;
	}

	//  Compute lift
	lift = 0.5*cl*density*vtotal*vtotal*mWingArea;

	//  Compute drag coefficient
	aspectRatio = mWingSpan*mWingSpan/mWingArea;
	cd = mCdp + cl*cl/(pi*aspectRatio*mEff);

	//  Compute drag force
	drag = 0.5*cd*density*vtotal*vtotal*mWingArea;

	//  Define some shorthand convenience variables
	//  for use with the rotation matrix.
	//  Compute the sine and cosines of the climb angle,
	//  bank angle, and heading angle;
	cosW = cos(mBank); 
	sinW = sin(mBank); 

	if( vtotal == 0.0 )
	{
		cosP = 1.0;
		sinP = 0.0;
	}
	else
	{
		cosP = vh/vtotal;  
		sinP = vz/vtotal;  
	}

	if( vh == 0.0 )
	{
		cosT = 1.0;
		sinT = 0.0;
	}
	else
	{
		cosT = vx/vh;
		sinT = vy/vh;
	}

	//  Convert the thrust, drag, and lift forces into
	//  x-, y-, and z-components using the rotation matrix.
	Fx = cosT*cosP*(thrust - drag) + 
		(sinT*sinW - cosT*sinP*cosW)*lift;
	Fy = sinT*cosP*(thrust - drag) + 
		(-cosT*sinW - sinT*sinP*cosW)*lift;
	Fz = sinP*(thrust - drag) + cosP*cosW*lift;

	//  Add the gravity force to the z-direction force.
	Fz = Fz + mMass*G;

	//  Since the plane can't sink into the ground, if the
	//  altitude is less than or equal to zero and the z-component
	//  of force is less than zero, set the z-force
	//  to be zero.
	if( z <= 0.0f && Fz <= 0.0f )
	{
		Fz = 0.0f;
	}

	//  Load the right-hand sides of the ODE's
	dq[0] = ds*(Fx/mMass);
	dq[1] = ds*vx;
	dq[2] = ds*(Fy/mMass);
	dq[3] = ds*vy;
	dq[4] = ds*(Fz/mMass);
	dq[5] = ds*vz;

	return;
}

double Airplane::GetBank()
{
	return mBank;
}

D3DXVECTOR3 Airplane::GetLook()
{
	return mLook;
}

void Airplane::Move( float dt )
{
	mPosition = BT2DX_VECTOR3( mPlaneChassis->getWorldTransform().getOrigin() );
	//  Get the initial values from the TextBox.
	//  The bank angle is converted to radians. The angle of
	//  attack is not converted to radians because the Cl-alpha
	//  equations use alpha in units of degrees.
	//////////////double throttle = (double)throttleTrackBar.Value/100.0;
	//////////////double alpha = (double)alphaTrackBar.Value;
	//////////////double bank = (double)bankTrackBar.Value*Math.PI/180.0;

	//////////////string flap = (string)flapComboBox.SelectedItem;
	if( GetAsyncKeyState( VK_UP ) & 0x8000 )
	{
		mAlpha -= dt * 5.0f;
		if( mAlpha <= -15.0f )
			mAlpha = -15.0f;
	}
	if( GetAsyncKeyState( VK_DOWN ) & 0x8000 )
	{
		mAlpha += dt * 5.0f;
		if( mAlpha >= 20.0f )
			mAlpha = 20.0f;
	}
	if( GetAsyncKeyState( VK_LEFT ) & 0x8000 )
	{
		mBank += dt / 2.0f;
		if( mBank >= 50.0f * ( PI / 180.0f ) )
			mBank = 50.0f * ( PI / 180.0f );
	}
	if( GetAsyncKeyState( VK_RIGHT ) & 0x8000 )
	{
		mBank -= dt / 2.0f;
		if( mBank <= -50.0f * ( PI / 180.0f ) )
			mBank = -50.0f * ( PI / 180.0f );
	}

	if( GetAsyncKeyState( 'T' ) & 0x8000 )
	{
		mThrottle += ( dt * 15.0f ) / 100.0f;
		if( mThrottle >= 1.0f )
			mThrottle = 1.0f;
	}
	if( GetAsyncKeyState( 'G' ) & 0x8000 )
	{
		mThrottle -= ( dt * 15.0f ) / 100.0f;
		if( mThrottle <= 0.0f )
			mThrottle = 0.0f;
	}

	//////plane.Throttle = throttle;
	//////plane.Alpha = alpha;
	//////plane.Bank = bank;
	//////plane.Flap = flap;

	//  Update the location and velocity of the airplane.
	UpdateLocationAndVelocity( dt );
}

void Airplane::Reset()
{
	mFlap = "0";         //  Flap setting

	mNumEqns = 6;

	mS = 0.0f;       //  time = 0.0
	mQ[0] = 0.0f;   //  vx0 = 0.0
	mQ[1] = 0.0f;   //  x0 = 0.0
	mQ[2] = 0.0f;   //  vy0 = 0.0
	mQ[3] = 0.0f;   //  y0 = 0.0
	mQ[4] = 0.0f;   //  vz0 = 0.0
	mQ[5] = 0.0f;   //  z0 = 0.0

	mThrottle = 0.0f;
	mAlpha = 0.0f;
	mBank = 0.0f;
}

float Airplane::GetWidth( UINT meshIndex )
{
	D3DXVECTOR3 min;
	D3DXVECTOR3 max;

	mChassisMesh->GetAABB( min, max );

	return abs( max.x - min.x );
}

float Airplane::GetHeight( UINT meshIndex )
{
	D3DXVECTOR3 min;
	D3DXVECTOR3 max;

	mChassisMesh->GetAABB( min, max );

	return abs( max.y - min.y );
}

float Airplane::GetLength( UINT meshIndex )
{
	D3DXVECTOR3 min;
	D3DXVECTOR3 max;

	mChassisMesh->GetAABB( min, max );

	return abs( max.z - min.z );
}

void Airplane::GetGround()
{
	D3DXVECTOR3 rayOrigin( mPosition.x, mPosition.y - ( GetHeight() / 2.0f ), mPosition.z );
	D3DXVECTOR3 rayDir( 0.0f, -1.0f, 0.0f );

	//// Tranform to world space.
	//D3DXMATRIX V = mWorld;

	//D3DXMATRIX inverseV;
	//D3DXMatrixInverse(&inverseV, 0, &V);

	//D3DXVec3TransformCoord(&rayOrigin, &rayOrigin, &inverseV);
	//D3DXVec3TransformNormal(&rayDir, &rayDir, &inverseV);

	// Transform to the mesh's local space.
	D3DXMATRIX gWorld;
	D3DXMatrixIdentity( &gWorld );
	D3DXMATRIX R;
	D3DXMATRIX T;
	D3DXMatrixRotationYawPitchRoll( &R, mGroundRotation.y, mGroundRotation.x, mGroundRotation.z );  
	D3DXMatrixTranslation( &T, mGroundPosition.x, mGroundPosition.y, mGroundPosition.z );
	gWorld = R * T;

	D3DXMATRIX inverseW;
	D3DXMatrixInverse(&inverseW, 0, &gWorld);

	D3DXVec3TransformCoord(&rayOrigin, &rayOrigin, &inverseW);
	D3DXVec3TransformNormal(&rayDir, &rayDir, &inverseW);

	ID3DX10Mesh* d3dxmesh = mGroundMesh->d3dxMesh();

	UINT hitCount;
	UINT pickedTriangle;
	float u,v,t;
	ID3D10Blob* allHits;
	d3dxmesh->Intersect(&rayOrigin, &rayDir, &hitCount, 
		&pickedTriangle, &u, &v, &t, &allHits);

	if( t <= 0.1f  && t >= -0.1f )
		mGroundDistance = 0.0f;
	else
		mGroundDistance = t;
}

void Airplane::SetGroundPosition( D3DXVECTOR3 pos )
{
	mGroundPosition = pos;
}

void Airplane::SetGroundRotation( D3DXVECTOR3 angle )
{
	mGroundRotation = angle;
}

void Airplane::Draw( D3DXMATRIX& lightViewProj )
{
	Vehicle::Draw( lightViewProj );

	// If the rigid body is a compound shape, get the child shapes and draw each shape
	// separately.
	btTransform cTrans;
	cTrans.setIdentity();
	cTrans.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) );
	btTransform trans = mPlaneChassis->getWorldTransform() * cTrans;
	mChassisMesh->Draw( mWorld, lightViewProj );
}

void Airplane::DrawToShadowMap( ID3D10EffectMatrixVariable* mfxBuildShadowMapLightWVPVar,
	ID3D10EffectShaderResourceVariable* mfxBuildShadowMapDiffuseMapVar,
	ID3D10EffectPass* pass,	D3DXMATRIX& lightViewProj )
{
	Vehicle::DrawToShadowMap( mfxBuildShadowMapLightWVPVar, mfxBuildShadowMapDiffuseMapVar,
		pass, lightViewProj );
}